% Smith form
% V*D*U=A

function [U, D, V]=smith_polymat(A,tol)

rho=1+tol;

tol2=1e-10;

[n, m]=size(A);

for i=1:n
    for j=1:n
        if i==j,
            unitmat_n{i,j}=1;
        else
            unitmat_n{i,j}=0;
        end
    end
end

for i=1:m
    for j=1:m
        if i==j,
            unitmat_m{i,j}=1;
        else
            unitmat_m{i,j}=0;
        end
    end
end

U=unitmat_m;
V=unitmat_n;

cont=0;
maxoper=200;

for i=1:n-1
    
    T1=unitmat_m;
    T1{i,i}=1/(A{i,i}(1));
    A=matmult(A,T1);
       
    flag_ii_div=1;
    
    while flag_ii_div

        flag_ii_z=1;

        while flag_ii_z

            flag_row=1;

            while flag_row;
                
                nzcount=0;

                for j=i+1:n   % vamos haciendo ceros en la fila cuando el grado es menor
                    % y dividiendo cuando el grado es mayor
                    
                    if abs_r(A{i,j})>tol,    % si el elemento i,j es distinto de cero

                        nzcount=nzcount+1;
                        
                        A=trimpolmat(A,tol2);

                        [q, r]=deconv(A{i,j},A{i,i});


                        if abs_r(r)<tol,    % si A(i,i) divide a A(i,j)
                            
                            T1=unitmat_m;           % construimos matriz correspondiente a 
                            T1{i,j}=-q;        % la operaci�n elemental que consiste en sumar 
                            T1inv=unitmat_m;        % la columna i multiplicada por q a la columna j, 
                            T1inv{i,j}=q;           % es decir, anulamos A(i,j)

                            A_ant=A;
                            A=matmult(A,T1);        % y la aplicamos
                            
                            U=matmult(T1inv,U);
                            
                            cont=cont+1;
                            if cont>maxoper,
                                error('smith_polymath:MaxOperExceeded','Max Oper Exceeded');
                            end
                            
                        else
                            
                            T1=unitmat_m;
                            T1{i,j}=-q;
                            T1inv=unitmat_m;
                            T1inv{i,j}=q;
                            T2=unitmat_m;

                            T2{i,i}=0;
                            T2{j,j}=0;
                            T2{i,j}=1;
                            T2{j,i}=1;

                            A_ant=A;
                            A=matmult(A,matmult(T1,T2));

                            U=matmult(T2',matmult(T1inv,U));
                            
                            cont=cont+1;
                            if cont>maxoper,
                                error('smith_polymath:MaxOperExceeded','Max Oper Exceeded');
                            end
                            
                        end

                    end

                end

                if nzcount==0,
                    flag_row=0;
                end

            end

            flag_col=1;

            while flag_col

                nzcount=0;

                for j=i+1:n

                    if abs_r(A{j,i})>tol,

                        nzcount=nzcount+1;
                        
                        A=trimpolmat(A,tol2);    

                        [q, r]=deconv(A{j,i},A{i,i});

                        if abs_r(r)<tol,

                            T1=unitmat_n;
                            T1{j,i}=-q;
                            T1inv=unitmat_n;
                            T1inv{j,i}=q;

                            A_ant=A;
                            A=matmult(T1,A);
                            
                            V=matmult(V,T1inv);
                            
                            cont=cont+1;
                            if cont>maxoper,
                                error('smith_polymath:MaxOperExceeded','Max Oper Exceeded');
                            end

                        else

                            T1=unitmat_n;
                            T1{j,i}=-q;
                            T1inv=unitmat_n;
                            T1inv{j,i}=q;
                            
                            T2=unitmat_n;

                            T2{i,i}=0;
                            T2{j,j}=0;
                            T2{i,j}=1;
                            T2{j,i}=1;

                            A_ant=A;
                            A=matmult(T2,matmult(T1,A));

                            V=matmult(V,matmult(T1inv,T2'));
                            
                            cont=cont+1;
                            if cont>maxoper,
                                error('smith_polymath:MaxOperExceeded','Max Oper Exceeded');
                            end

                        end

                    end

                end
                if nzcount==0,
                    flag_col=0;
                end

            end % end of while column

            flag_ii_z=0;

            for j=i+1:n
                if abs_r(A{i,j})>tol || abs_r(A{j,i})>tol,
                    flag_ii_z=1;
                end
            end

        end %while flag_ii_z
        
        % comprueba si A(i,i) divide a toda la submatriz
        
        flag_ii_div=0;
        
        for j=i+1:n
            for k=i+1:n
                
                A=trimpolmat(A,tol2);
                
                [q, r]=deconv(A{j,k},A{i,i});
                
                if abs_r(r)>tol, % si hay alguno que no divide
                    flag_ii_div=1;      % hay que seguir
                                        
                    T1=unitmat_n;
                    T1{i,j}=1;
                    T1inv=unitmat_n;
                    T1inv{i,j}=-1;

                    A_ant=A;
                    A=matmult(T1,A);

                    V=matmult(V,T1inv);
                    
                    cont=cont+1;
                    if cont>maxoper,
                        error('smith_polymath:MaxOperExceeded','Max Oper Exceeded');
                    end
                    
                    break;
                end
                                    
            end
            
            if flag_ii_div==1,
                break;
            end
            
        end
        
    end % while flag_ii_div

end

D=A;
